//
// Created by xuan on 23-12-12.
//

#include "UPRE_GYRO100A.h"
#include "usart.h"

uint8_t gyro_receive;
struct Gyro gyro = {


        .State=0,
        .Angle_pitch=0,					//俯仰角
        .Angle_roll=0,					//横滚角
        .Angle_azimuth=0,				//方位角
        .W_x=0,		       			  //惯导角速率
        .W_y=0,
        .A_y=0,
        .W_z=0,
        .A_x=0,									//惯导加速度
        .A_z=0,
};
struct GYRO_100A gyro_100A = {0};


void GYRO_A100_Init(void)
{
    HAL_UART_Receive_IT(&huart2, &gyro_receive,1);
}

inline static bool checkGyroDate(void)
{
    return (gyro_100A.gyro_buffer[1] ^
            gyro_100A.gyro_buffer[2] ^
            gyro_100A.gyro_buffer[3] ^
            gyro_100A.gyro_buffer[4] == gyro_100A.gyro_buffer[5]);
}

static void getGyroYaw(void)
{
    if ((gyro_100A.gyro_buffer[1] & 0x40U) == 0x00U)
    {
        gyro_100A.gyro_original_angle = (gyro_100A.gyro_buffer[4]      ) |
                                        (gyro_100A.gyro_buffer[3] << 7 ) |
                                        (gyro_100A.gyro_buffer[2] << 14) |
                                        (gyro_100A.gyro_buffer[1] << 21) ;

        gyro_100A.gyro_original_temperature = (gyro_100A.gyro_buffer[7]) |
                                              (gyro_100A.gyro_buffer[6] << 7);

    }
    else
    {
        gyro_100A.gyro_original_angle = (gyro_100A.gyro_buffer[4]      ) |
                                        (gyro_100A.gyro_buffer[3] << 7 ) |
                                        (gyro_100A.gyro_buffer[2] << 14) |
                                        (gyro_100A.gyro_buffer[1] << 21) |
                                        0xf0000000U;

        gyro_100A.gyro_original_temperature = (gyro_100A.gyro_buffer[7])     |
                                              (gyro_100A.gyro_buffer[6] << 7)|
                                              0xffffc0000U;
    }

    gyro_100A.gyro_temperature = gyro_100A.gyro_original_temperature * GYRO_TEMPERATURE_COEFFICIENT;

    gyro_100A.gyro_angle_s = gyro_100A.gyro_original_angle / GYRO_FACTOR;
    gyro_100A.gyro_angle   = gyro_100A.gyro_angle + (gyro_100A.gyro_angle_s * GYRO_TIME) - GYRO_EARTH_SPEED;
    gyro_100A.gyro_angleDate     = gyro_100A.gyro_angle /*- gyro_100A.compensation*/;
    gyro.Angle_azimuth = gyro_100A.gyro_angleDate;
#if Printf_Gyro == 1
    printf("gyro_yaw:%f\r\n",gyro.Angle_azimuth);
#endif
//    return gyro_100A.gyro_angleDate;
}

#if GYRO_RECIVE == 1
void get_gyroValue(void)
{

    if(gyro_receive == 0x80)
    {
        gyro_100A.gyro_location = 1;
    }
    else
    {
        if(gyro_100A.gyro_location > 0 && gyro_100A.gyro_location < 8)
        {
            gyro_100A.gyro_buffer[gyro_100A.gyro_location++] = gyro_receive;
        }
        if(gyro_100A.gyro_location > 7)
        {
            if (checkGyroDate())
            {
                getGyroYaw();
                gyro_100A.gyro_location = 0;
            }
            else
            {
//                gyro_100a.gyro_error_num++;
                gyro_100A.gyro_location = 0;
            }
        }

    }
//    static uint8_t N = 0;
//    static uint8_t Gyro_receive = 0;
//
//    gyro_100A.gyro_buffer[N] = Gyro_receive;
//    if(N == 7 && checkGyroDate())
//    {
//        if(Gyro_receive == 0x80 )
//        {
//            N = 0;
//        }
//    }
////    printf("0:%x\r\n",gyro_100A.gyro_buffer[0]);
//    N++;
    HAL_UART_Receive_IT(&huart2, &gyro_receive,1);
}
#endif
#if GYRO_RECIVE == 8

void get_gyroValue(void)
{

    if(gyro_receive[0] == 0x80)
    {
        for (int i = 0; i < 7; ++i) {
            gyro_100A.gyro_buffer[i] = gyro_receive[i];
        }
        if (checkGyroDate())
        {
            getGyroYaw();
        }
        else
        {
//            gyro_100a.gyro_error_num++;
        }
    }

    HAL_UART_Receive_IT(&huart2, &gyro_receive[8],8);
}

#endif

static float operation_yaw(float Iint_yaw)
{
    static float yaw = 0, last_yaw = 0, delta_yaw = 0, final_yaw = 0;
    yaw = Iint_yaw;
    delta_yaw = yaw - last_yaw;
    final_yaw += delta_yaw;
    last_yaw = final_yaw;
    return final_yaw;
}

float returnyaw(void)
{
    return (operation_yaw(gyro_100A.gyro_angleDate));
}
